#include "global_routing.h"

/*定义起点位置的回调函数*/
void GlobalRouting::start_pose_call_back(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
  if (Start_dynamic == "0") // Start_dynamic == "0"代表车已经到达终点,即准备开始新的规划路线
  {
    is_start_pose_set = true;
    start_pose_.position.x = msg->pose.pose.position.x;
    start_pose_.position.y = msg->pose.pose.position.y;
    start_pose_.orientation = msg->pose.pose.orientation;

    if (carla_simulation == false)
    {
      std::cout << "start_x:" << msg->pose.pose.position.x << std::endl;
      std::cout << "start_y:" << msg->pose.pose.position.y << std::endl;
      std::cout << "theta_start:" << tf::getYaw(msg->pose.pose.orientation) << "\n";
      publish_car_start_pose(start_pose_);

      // 起点当作参考线起点
      // hdmap_way_points(0, 0) = start_pose_.position.x;
      // hdmap_way_points(0, 1) = start_pose_.position.y;
      // hdmap_way_points(0, 2) = 0;

      // 显示，方便
      visualization_msgs::Marker marker;
      marker.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
      marker.header.frame_id = Frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = "basic_shapes";
      marker.id = obstacle_id++; // 注意了
      marker.type = visualization_msgs::Marker::SPHERE;
      marker.action = visualization_msgs::Marker::ADD;
      marker.pose.position.x = start_pose_.position.x;
      marker.pose.position.y = start_pose_.position.y;
      marker.pose.position.z = 0;
      marker.scale.x = 1;
      marker.scale.y = 1;
      marker.scale.z = 0;
      marker.color.r = 1.0f;
      marker.color.g = 0.0f;
      marker.color.b = 0.0f;
      marker.color.a = 1.0;
      marker.lifetime = ros::Duration();
      obstacle_points.markers.push_back(marker);
      obstacle_points_pub_.publish(obstacle_points);
    }
  }

  else
  {
    ROS_WARN("ego vehicle is running!");
  }
}

/*定义终点位置的回调函数*/
void GlobalRouting::goal_pose_call_back(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  // Start_dynamic == "0"代表车已经到达终点,即准备开始新的规划路线
  if (Start_dynamic == "0" && is_start_pose_set == true)
  {
    is_goal_pose_set = true;
    goal_pose_.position.x = msg->pose.position.x;
    goal_pose_.position.y = msg->pose.position.y;
    goal_pose_.orientation = msg->pose.orientation;
  }
  else if (Start_dynamic == "0" && is_start_pose_set == false)
  {
    ROS_WARN("Please set the starting point!");
  }
  else if (Start_dynamic != "0")
  {
    ROS_WARN("ego vehicle is running!");
  }
  if (carla_simulation == true)
  {
    // 显示，方便
    visualization_msgs::Marker marker;
    marker.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 3.14);
    marker.header.frame_id = Frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.id = 0; // 注意了
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = goal_pose_.position.x;
    marker.pose.position.y = goal_pose_.position.y;
    marker.pose.position.z = 2;
    marker.scale.x = 1;
    marker.scale.y = 1;
    marker.scale.z = 0;
    marker.color.r = 1.0;
    marker.color.g = 0.0;
    marker.color.b = 0.0;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();
    goal_point_pub_.publish(marker);
  }
}

/*读回carla全局规划发过来的导航点数据 , 作为参考线的路径点*/
void GlobalRouting::get_Waypoints_From_Carla(const nav_msgs::Path &waypoints)
{
  // 重置原来的参考路径点
  hdmap_way_points = Eigen::MatrixXd::Zero(waypoints.poses.size(), 3);

  for (int i = 0; i < waypoints.poses.size(); i++)
  {
    hdmap_way_points(i, 0) = waypoints.poses[i].pose.position.x;
    hdmap_way_points(i, 1) = waypoints.poses[i].pose.position.y;
    hdmap_way_points(i, 2) = 0;
  }
}

/*读回ros odom坐标系数据 , 接收车的里程信息，控制车的移动*/
void GlobalRouting::odom_call_back(const nav_msgs::Odometry &msg)
{
  car_odom_ = msg; // 车的里程信息，就是位置信息

  // 坐标转换
  geometry_msgs::Quaternion odom_quat = msg.pose.pose.orientation;
  tf::Quaternion quat;
  tf::quaternionMsgToTF(odom_quat, quat);

  // 根据转换后的四元数，获取roll pitch yaw
  double roll, pitch, yaw;
  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

  cur_pose.x = msg.pose.pose.position.x;
  cur_pose.y = msg.pose.pose.position.y;
  cur_pose.yaw = yaw;

  cur_pose.vx = msg.twist.twist.linear.x;
  cur_pose.vy = msg.twist.twist.linear.y;
  cur_pose.v = std::sqrt(cur_pose.vx * cur_pose.vx + cur_pose.vy * cur_pose.vy);

  cur_pose.z = msg.pose.pose.position.z;
}

/*获取局部规划来的航向角*/
void GlobalRouting::Vehival_Theta(const geometry_msgs::PoseArray &theta)
{
  thetas.clear();
  if (theta.poses.size() > 0)
  {
    for (const auto &pose : theta.poses) {
      thetas.push_back(pose.position.x);
    }
  }
}

/*获取局部规划来的曲率*/
void GlobalRouting::Vehival_Kappa(const geometry_msgs::PoseArray &kappa)
{
  kappas.clear();
  if (kappa.poses.size() > 0)
  {
    for (const auto &pose : kappa.poses) {
      kappas.push_back(pose.position.x);
    }
  }
}

/*获取局部轨迹的信号*/
void GlobalRouting::Vehival_Go(const std_msgs::String::ConstPtr &go)
{
  Start_dynamic = go->data.c_str();
}

/*获取局部轨迹*/
void GlobalRouting::Vehival_Traj(const waypoint_msgs::WaypointArray::ConstPtr &msg)
{
  dynamic_points.poses.clear();
  dynamic_points.header.frame_id = Frame_id;
  dynamic_points.header.stamp = ros::Time::now();
  speeds.clear();
  local_waypoints.resize(msg->waypoints.size());

  for (int i = 0; i < msg->waypoints.size(); i++)
  {
    // 为carla的控制模块准备变量
    car_state temp_point;
    temp_point.x = msg->waypoints[i].pose.pose.position.x;
    temp_point.y = msg->waypoints[i].pose.pose.position.y;
    temp_point.v = msg->waypoints[i].twist.twist.linear.x; //速度
    local_waypoints[i] = temp_point;

    geometry_msgs::PoseStamped pose_stamp;
    pose_stamp.header.frame_id = Frame_id;
    pose_stamp.header.stamp = ros::Time::now();
    pose_stamp.pose.position.x = msg->waypoints[i].pose.pose.position.x;
    pose_stamp.pose.position.y = msg->waypoints[i].pose.pose.position.y;
    pose_stamp.pose.position.z = 0;
    dynamic_points.poses.push_back(pose_stamp);
    speeds.push_back(msg->waypoints[i].twist.twist.linear.x); //速度
  }
}

/*发布车辆在路径的起始位置,传入起点*/
void GlobalRouting::publish_car_start_pose(const geometry_msgs::Pose &start_pose)
{
  geometry_msgs::Vector3 state;
  // get 起始位置的yaw
  double set_vehicle_yaw = tf::getYaw(start_pose.orientation);
  // get vehicle start pose
  state.x = start_pose.position.x;
  state.y = start_pose.position.y;
  state.z = set_vehicle_yaw;
  // 发布车的起点位置
  vehicle_start_pose_pub_.publish(state);
}

// 判断车是否到了路径的尽头，传入 终点，车里程
bool GlobalRouting::Vehical_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &odom)
{
  // 获取车的里程位置
  double x = odom.pose.pose.position.x;
  double y = odom.pose.pose.position.y;
  // 获取路径点的最后一个位置
  double xt = goal_point.position.x;
  double yt = goal_point.position.y;
  // 判断如果两点坐标接近

  double dis = sqrt((x - xt) * (x - xt) + (y - yt) * (y - yt));
  if (dis < goal_distanse)
  {
    return true;
  }
  return false;
}

void GlobalRouting::create_map()
{
  // std::vector<double> map_1_x = {48.6505, 21.935, 0.32034, -4.89225, -8.76959, -14.8733, -20.663, -31.3403, -41.9258,
  //                                -48.5286, -54.6687, -59.3578, -61.0144, -61.6068, -60.3871, -57.5776, -52.5193};
  // std::vector<double> map_1_y = {-52.271, -27.4259, -7.07192, -2.08155, 1.55404, 7.22386, 12.8774, 22.9359, 33.0932,
  //                                39.2203, 45.6366, 51.9786, 57.4503, 63.7577, 82.018, 98.506, 125.59};

  // std::vector<double> map_1_x = {-41.9258,
  //                                -48.5286, -54.6687, -59.3578, -61.0144, -61.6068, -60.3871, -57.5776, -52.5193};
  // std::vector<double> map_1_y = {33.0932,
  //                                39.2203, 45.6366, 51.9786, 57.4503, 63.7577, 82.018, 98.506, 125.59};
  /* std::vector<double> map_1_x = {48.6505,  21.935,  0.32034, -8.76959, -151.914, -340.843, -2124.25};
  std::vector<double> map_1_y = {-52.271,  -27.4259,  -7.07192,  1.55404, 142.054, 321.606, 2187.95}; */
  std::vector<double> map_1_x = {48.6505,  21.935,  0.32034, -8.76959, -151.914, -340.843};
  std::vector<double> map_1_y = {-52.271,  -27.4259,  -7.07192,  1.55404, 142.054, 321.606};


  // std::vector<double> map_1_x = {48.6505,  21.935,  0.32034, -8.76959, -151.914};
  // std::vector<double> map_1_y = {-52.271,  -27.4259,  -7.07192,  1.55404};

  // std::vector<double> map_1_x = {48.6505, 21.935};
  // std::vector<double> map_1_y = {-52.271, -27.4259};

  hdmap_way_points = Eigen::MatrixXd::Zero(map_1_x.size(), 3); // 初始化零矩阵
  for (int i = 0; i < map_1_x.size(); i++)
  {
    visualization_msgs::Marker marker;
    marker.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
    marker.header.frame_id = Frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.id = i; // 注意了
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = map_1_x[i];
    marker.pose.position.y = map_1_y[i];
    marker.pose.position.z = 0;
    marker.scale.x = 1;
    marker.scale.y = 1;
    marker.scale.z = 0;
    marker.color.r = 1.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();
    map_points.markers.push_back(marker);
  }

  // 加入起点的话i = 1
  for (int i = 0; i < map_1_x.size(); i++)
  {
    hdmap_way_points(i, 0) = map_1_x[i];
    hdmap_way_points(i, 1) = map_1_y[i];
    hdmap_way_points(i, 2) = 0;
  }

  sleep(2);
  map_points_pub_.publish(map_points);
}

/*默认构造函数：规划函数,初始化参数*/
GlobalRouting::GlobalRouting()
{
  ros::NodeHandle n;
  // 参数获取
  n.param("use_what_planner", use_what_planner, 1);       // 局部轨迹生成方式选择
  n.param("use_what_controller", use_what_controller, 1); // 控制算法的选择
  n.param("carla_simulation", carla_simulation, false);   // 是否跟carla联合仿真
  n.param("parking_mode", parking_simulation, false);
  n.param("goal_distanse", goal_distanse, 0.5);
  n.param<string>("role_name", role_name, "ego_vehicle");

  start_pose_subscriber_ = n.subscribe("/initialpose", 10, &GlobalRouting::start_pose_call_back, this);
  goal_pose_subscriber_ = n.subscribe("/move_base_simple/goal", 10, &GlobalRouting::goal_pose_call_back, this);

  //
  is_open_scense = false;
  if (use_what_planner == 4 || use_what_planner == 6 || use_what_planner == 7)
  {
    is_open_scense = true;
  }

  // 回调
  if (carla_simulation == false)
  {
    odom_sub_ = n.subscribe("/xsj/odom", 10, &GlobalRouting::odom_call_back, this);
    map_points_pub_ = n.advertise<visualization_msgs::MarkerArray>("/xsj/maps/map_points", 10);               // ros仿真下的地图路线点显示
    obstacle_points_pub_ = n.advertise<visualization_msgs::MarkerArray>("/xsj/obstacle/obstacle_points", 10); // 障碍物的顶点设置
    control_data_pub_ = n.advertise<geometry_msgs::Vector3>("/xsj/car/control_car", 10);                      // ros仿真时发布车的控制数据
    vehicle_start_pose_pub_ = n.advertise<geometry_msgs::Vector3>("/xsj/car/car_start_pose", 10);             // 发布车的起点位置

    // 打印控制器
    if (use_what_controller == 1)
      ROS_WARN("Controller: stanley");
    else if (use_what_controller == 2)
      ROS_WARN("Controller: lqr");
    else if (use_what_controller == 3)
      ROS_WARN("Controller: pure_pursuit");
    else if (use_what_controller == 4)
      ROS_WARN("Controller: pid");
    else if (use_what_controller == 5)
      ROS_WARN("Controller: mpc");
    Mpc.Init();
  }
  else
  {
    n.param<string>("control_method", control_method, "LQR");
    n.param<double>("k_pure", k_pure, 1.0); // PurePuresuit增益系数
    n.param<double>("k_cte", k_cte, 3.0);   // Stanley增益系数
    // 纵向PID参数
    n.param<double>("kp", kp, 0.85);
    n.param<double>("ki", ki, 0.02);
    n.param<double>("kd", kd, 0.1);
    // LQR_dynamics Q  R矩阵权重
    n.param<double>("Q_ed", Q_ed, 67.0);
    n.param<double>("Q_ed_dot", Q_ed_dot, 1.0);
    n.param<double>("Q_ephi", Q_ephi, 70.0);
    n.param<double>("Q_ephi_dot", Q_ephi_dot, 1.0);
    n.param<double>("R_value", R_value, 35.0);
    // LQR_kinematics Q  R矩阵权重
    n.param<double>("Q_ex_k", Q_ex_k, 67.0);
    n.param<double>("Q_ed_k", Q_ed_k, 67.0);
    n.param<double>("Q_ephi_k", Q_ephi_k, 70.0);
    n.param<double>("R_value_k", R_value_k, 35.0);

    waypoints_sub_ = n.subscribe("/carla/" + role_name + "/waypoints", 10, &GlobalRouting::get_Waypoints_From_Carla, this);
    odom_sub_ = n.subscribe("/carla/" + role_name + "/odometry", 10, &GlobalRouting::odom_call_back, this);
    goal_point_pub_ = n.advertise<visualization_msgs::Marker>("/xsj/goal_points", 10);
    control_cmd_pub_ = n.advertise<carla_msgs::CarlaEgoVehicleControl>("/carla/" + role_name + "/vehicle_control_cmd", 10);
  }
  // 局部规划订阅
  vehival_theta = n.subscribe("/xsj/planning/dynamic_paths_t", 10, &GlobalRouting::Vehival_Theta, this);
  vehival_kappa = n.subscribe("/xsj/planning/dynamic_paths_k", 10, &GlobalRouting::Vehival_Kappa, this);
  vehival_traj = n.subscribe("/xsj/planning/dynamic_waypoints", 10, &GlobalRouting::Vehival_Traj, this);
  vehival_go = n.subscribe("/xsj/planning/Start_Dynamic", 10, &GlobalRouting::Vehival_Go, this);

  // 初始化 标志位：
  is_start_pose_set = false;
  is_goal_pose_set = false;
  is_begin_reference = false;
  is_vehical_stop_set = true;
  Start_dynamic = "0";
  obstacle_id = 0;

  // 车的初始值设置
  car_speed = 0;

  // 创建map
  if (carla_simulation == false)
  {
    create_map();
  }
  // 创建线程
  routing_thread_ = new boost::thread(boost::bind(&GlobalRouting::thread_routing, this));
}

/**
   * @brief 求解离散LQR矩阵
   *
   * @param vx 纵向速度
   * @return Eigen::MatrixXd
   */
Eigen::MatrixXd GlobalRouting::calc_dlqr(double vx, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)
{
  vx = vx + 0.00001; // 防止速度为0时相除出错
  // A矩阵
  /*
    A matrix (Gear Drive)
    [0.0,                             1.0,                           0.0,                                            0.0;
     0.0,          (cf + cr) / (mass * vx),            -(cf + cr) / mass,              (lf * cf - lr * cr) / (mass * vx);
     0.0,                             0.0,                           0.0,                                            1.0;
     0.0, (lf * cf - lr * cr) / (Iz * vx),     -(lf * cf - lr * cr) / Iz,       (lf * lf * cf + lr * lr * cr) / (Iz * vx);]
    */
  Eigen::Matrix4d A;
  A << 0.0, 1.0, 0.0, 0.0,
      0.0, (cf + cr) / (mass * vx), -(cf + cr) / mass, (lf * cf - lr * cr) / (mass * vx),
      0.0, 0.0, 0.0, 1.0,
      0.0, (lf * cf - lr * cr) / (Iz * vx), -(lf * cf - lr * cr) / Iz, (lf * lf * cf + lr * lr * cr) / (Iz * vx);

  // B矩阵 ：B = [0.0, -cf / mass, 0.0, -lf * cf / Iz]^T

  Eigen::Matrix<double, 4, 1> B;
  B << 0.0, -cf / mass, 0.0, -lf * cf / Iz;

  /***********************************离散化状态方程**************************************/
  double ts = 0.01;
  // 单位矩阵
  Eigen::Matrix4d eye;
  eye.setIdentity(4, 4);
  // 离散化A,B矩阵
  Eigen::Matrix4d Ad;
  Ad = (eye - ts * 0.5 * A).inverse() * (eye + ts * 0.5 * A);
  Eigen::Matrix<double, 4, 1> Bd;
  Bd = B * ts;

  /***********************************求解Riccati方程**************************************/
  int max_iteration = 100; // 设置最大迭代次数
  int tolerance = 0.001;   // 迭代求解精度

  Eigen::Matrix4d P = Q;
  Eigen::Matrix4d P_next;
  for (int i = 0; i < max_iteration; i++)
  {
    P_next = Ad.transpose() * P * Ad -
             Ad.transpose() * P * Bd * (R + Bd.transpose() * P * Bd).inverse() * Bd.transpose() * P * Ad + Q;
    if (fabs((P_next - P).maxCoeff()) < tolerance)
    {
      break;
    }
    P = P_next;
  }
  // 求解k值
  Eigen::MatrixXd k;
  k = (R + Bd.transpose() * P * Bd).inverse() * Bd.transpose() * P * Ad;
  return k;
}

/**
   * @brief 离线求解LQR表
   *
   * @param vx
   */
void GlobalRouting::CreateLqrOffline(const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R)
{
  lqr_k_table.clear();
  for (double v = 0; v < 30.0; v += 0.2)
  {
    lqr_k_table.push_back(calc_dlqr(v, Q, R));
  }
}

/*析构函数：释放线程*/
GlobalRouting::~GlobalRouting()
{
  delete routing_thread_;
}

// 开车，控制车的行使
void GlobalRouting::thread_routing()
{
  double frequency = 10.0;
  ros::NodeHandle n;
  ros::Rate loop_rate(frequency);
  double turn_angle = 0;
  int out_index_ = 0;
  std::vector<double> prev_p(2);
  Eigen::MatrixXd Q;
  Eigen::MatrixXd R;
  // 动力学
  if (control_method == "LQR_dynamics")
  {
    // Q矩阵
    Q.setZero(4, 4);
    Q(0, 0) = Q_ed;
    Q(1, 1) = Q_ed_dot;
    Q(2, 2) = Q_ephi;
    Q(3, 3) = Q_ephi_dot;
    // R矩阵
    R.setZero(1, 1);
    R(0, 0) = R_value;
    CreateLqrOffline(Q, R); // 离线LQR
  }
  // 运动学
  else if (control_method == "LQR_kinematics")
  {
    // Q矩阵
    Q.setZero(3, 3);
    Q(0, 0) = Q_ex_k;
    Q(1, 1) = Q_ed_k;
    Q(2, 2) = Q_ephi_k;
    // R矩阵
    R.setZero(2, 2);
    R(0, 0) = R_value_k;
    R(1, 1) = R_value_k;
  }
  else
  {
    Q.setZero(4, 4);
    R.setZero(2, 2);
  }
  carla_pnc::Controller mc(control_method,
                           k_pure, k_cte,
                           Q, R,
                           kp, ki, kd,
                           lqr_k_table);

  vector<double> cmd(3);
  vector<double> pre_cmd(3);

  while (n.ok())
  {
    if (hdmap_way_points.rows() > 0 && is_start_pose_set == true &&
        is_goal_pose_set == true && is_begin_reference == false && parking_simulation == false && is_open_scense == false)
    {
      // 局部规划参考线:hdmap_way_points
      Rs.referenceLine_split(hdmap_way_points);
      is_begin_reference = true;
    }

    // is_start_pose_set== true已经设置起点
    // is_goal_pose_set== true已经设置终点
    // Start_dynamic 局部规划轨迹开始生成
    if (is_start_pose_set == true && is_goal_pose_set == true && Start_dynamic == "1")
    {
      is_vehical_stop_set = Vehical_Stop(goal_pose_, car_odom_); // 判断是否到达终点

      if (carla_simulation == false) // ros仿真
      {
        if (use_what_controller == 1){
          std::cout << "kappas:" << kappas.size() << std::endl;
          Stanly.stanley_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
        }
        else if (use_what_controller == 2){
          Lqr.lqr_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
        }
        else if (use_what_controller == 3)
          Pursuit.pure_pursuit_control(dynamic_points, car_odom_, car_speed, turn_angle, out_index_);
        else if (use_what_controller == 4)
          Pid.pid_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
        else if (use_what_controller == 5)
          Mpc.mpc_control(car_odom_, dynamic_points, kappas, thetas, speeds, turn_angle, out_index_);
        msg_ros.y = turn_angle;           // 转角
        if (is_vehical_stop_set == false) // 未到达终点
        {
          if (use_what_controller == 5)
          {
            car_speed = out_index_;
          }
          else
          {
            car_speed = speeds[out_index_];
          }
          msg_ros.x = car_speed; // ros仿真直接赋值速度
          control_data_pub_.publish(msg_ros);
          //ROS_INFO("speed: %2f, turn_angle: %2f", car_speed, turn_angle);
        }
        else // 已经到达终点
        {
          ROS_WARN("Arrive at goal");
          msg_ros.x = 0;
          control_data_pub_.publish(msg_ros);
          is_start_pose_set = false;
          is_goal_pose_set = false;
          is_begin_reference = false;
          Start_dynamic = "0";
          car_speed = 0;
        }
      }
      else // carla仿真
      {
        if (is_vehical_stop_set == false) // 未到达终点
        {
          double current_timestamp = ros::Time::now().toSec();
          double dist = hypot(cur_pose.x - prev_p[0], cur_pose.y - prev_p[1]);
          if (dist > 10.0)
          {
            mc.reset_all_previous();
            local_waypoints.clear();
          }
          prev_p[0] = cur_pose.x;
          prev_p[1] = cur_pose.y;

          /***********************************控制主体**************************************/
          if (local_waypoints.size() > 1)
          {
            mc.update_waypoints(local_waypoints);
            // cout<<"更新路径点成功"<<endl;
            mc.update_car_state(cur_pose, current_timestamp);
            // cout<<"更新状态成功"<<endl;
            mc.update_controls(frequency);
            // cout<<"更新控制成功"<<endl;
            cmd = mc.get_commands();
            pre_cmd = cmd;
          }
          else
          {
            cmd = {0, 0, 1.0};
          }
          if (parking_simulation == false)
            ROS_INFO("throttle: %2f, steer: %2f, brake: %2f", cmd[0], cmd[1], cmd[2]);

          /***********************************发布控制指令**************************************/
          carla_msgs::CarlaEgoVehicleControl control_cmd;
          control_cmd.throttle = cmd[0];
          control_cmd.steer = cmd[1];
          control_cmd.brake = cmd[2];
          control_cmd.hand_brake = false;
          control_cmd.reverse = false;
          control_cmd_pub_.publish(control_cmd);
        }
        else // 已经到达终点
        {
          ROS_WARN("Arrive at goal");
          carla_msgs::CarlaEgoVehicleControl control_cmd;
          control_cmd.throttle = 0;
          control_cmd.steer = 0;
          control_cmd.brake = 1;
          control_cmd.hand_brake = false;
          control_cmd.reverse = false;
          control_cmd_pub_.publish(control_cmd);
          is_start_pose_set = false;
          is_goal_pose_set = false;
          is_begin_reference = false;
          Start_dynamic = "0";
          car_speed = 0;
        }
      }
    }
    else if (is_start_pose_set == true && is_goal_pose_set == true && Start_dynamic == "2") // 没有轨迹强制停车
    {
      if (carla_simulation == false)
      {
        msg_ros.x = 0;
        control_data_pub_.publish(msg_ros);
        car_speed = 0;
      }
      else
      {
        carla_msgs::CarlaEgoVehicleControl control_cmd;
        control_cmd.throttle = 0;
        control_cmd.steer = 0;
        control_cmd.brake = 1;
        control_cmd.hand_brake = false;
        control_cmd.reverse = false;
        control_cmd_pub_.publish(control_cmd);
        car_speed = 0;
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
